#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <QString>
#include <cv.h>
#include <cv_bridge/CvBridge.h>
#include "pcl_ros/transforms.h"
#include "parameter_server.h"

int i=0;


void savingPcdCallback(const sensor_msgs::PointCloud2ConstPtr& cloud){
char filename[256];
  //FILE *file;
snprintf(filename, sizeof(filename),"offline_transformed_cloud_%d.pcd", i++);
//file = fopen(filename, "w+");

 pcl::PointCloud<pcl::PointXYZRGB> saving_cloud;
 pcl::fromROSMsg(*cloud, saving_cloud);
//pointcloud_type transformed_cloud;
 pcl::io::savePCDFileASCII(filename, saving_cloud);
 ROS_INFO("savingPCDFile");
}
	
void colorImageCallback(const sensor_msgs::ImageConstPtr& color_img_msg){
     
 sensor_msgs::CvBridge bridge;
 cv::Mat visual_img =  bridge.imgMsgToCv(color_img_msg, "rgb8");
     
}
	
int main (int argc, char** argv)
{
	ros::init(argc, argv, "saving_pcd");
	ros::NodeHandle nh;
	//ros::Subscriber saving_sub = nh.subscribe<sensor_msgs::PointCloud2>(
		//	"/rgbdslam/transformed_cloud", 20,
			//savingPcdCallback);
			
	ros::Subscriber saving_sub = nh.subscribe<sensor_msgs::PointCloud2>(
	   "/camera/rgb/points", 20,
			savingPcdCallback);
	
	//ros::Subscriber color_image_sub = nh.subscribe<sensor_msgs::Image>(
		//	"camera/rgb/image_color", 20,
			//colorImageCallback);
			

	ros::Rate loop_rate(10);
	
	while(ros::ok()){
     loop_rate.sleep();
     ros::spinOnce();
     }
}



